/*
 * @Author: Jiagang Chen
 * @Date: 2021-11-11 13:52:35
 * @LastEditors: Jiagang Chen
 * @LastEditTime: 2021-12-01 02:32:26
 * @Description: ...
 * @Reference: ...
 */

#ifndef SRC_IMU_CONVERTER_H
#define SRC_IMU_CONVERTER_H
#include <string>
#include "converter.h"

namespace kaist_publisher {

class ImuConverter : public Converter {
public:
    typedef std::shared_ptr<ImuConverter> Ptr;
    ImuConverter(ros::NodeHandle& nh, const std::string& dataset_dir,
                 const std::string& save_dir, const std::string& irp_topic,
                 const std::string& raw_topic, const std::string& mag_topic);
    virtual ~ImuConverter() = default;

    int Convert() override;

    std::string default_data_file = "sensor_data/xsens_imu.csv";

    ros::Publisher irp_publisher;
    ros::Publisher raw_publisher;
    ros::Publisher mag_publisher;

    int64_t Init_Publish();
    int64_t Publish();
    void Publish(int64_t& timestamp, sensor_msgs::Imu::Ptr& sensor_msgs_imu,
                 sensor_msgs::MagneticField::Ptr& sensor_msgs_mag);

    FILE* fp_;
    int64_t stamp_;
    double q_x_, q_y_, q_z_, q_w_, x_, y_, z_, g_x_, g_y_, g_z_, a_x_, a_y_,
        a_z_, m_x_, m_y_, m_z_;

private:
    std::string irp_topic_;
    std::string raw_topic_;
    std::string mag_topic_;
    std::string irp_bag_name_;
    std::string raw_bag_name_;
    std::string mag_bag_name_;
};

}  // namespace kaist_publisher
#endif  // SRC_IMU_CONVERTER_H
